
#ifndef _CHASSIS_TASK_H
#define _CHASSIS_TASK_H
#include "stm32f4xx_hal.h"
#include "FreeRTOS.h"
#include "cmsis_os2.h"
#include "semphr.h"
#include "queue.h"
#include "string.h"
#include <stdio.h>
#include <stdlib.h>

#include "bsp_DJMOTOR.h"
#include "bsp_servo.h"
#include "bsp_can.h"
#include "ctrl.h"
#include "bsp_vofa.h"
#include "bsp_led.h"
#include "arm_math.h"

/* 任务间通讯 */
#define BIT_1			0x00000010U
#define ARM_DONE	BIT_1 // 任务完成标志

#define Arm_Param_DeBug 	0 // 机械臂参数调试 PID参数

// 升降 (部分高度自行修改)
// M2006速度采集低通滤波
#define M2006_LOW_FILITER_PARAM				0.1f

#define ARM_VETIVCAL_START					0.3f // 起始高度
#define ARM_CATCH_FROM_TURNTABLE			2.1f // 调试()转盘处抓取物料
#define ARM_CATCH_PUT_FROM_MATERIAL_TRAY	2.f // 调试()从物料盘取和放物料

// 云台
#define GM6020_LOW_FILITER_PARAM0			0.1f // 低通滤波
#define GM6020_LOW_FILITER_PARAM1			0.1f // 低通滤波
#define ARM_YAW_START			7500 // // 起始位置(机械夹爪收起
#define ARM_YAW_LEFT			6000 // 机械臂处于左
#define ARM_YAW_MID				5500 // 中部
#define ARM_YAW_RIGHT			3000 // 右
#define ARM_YAW_PUT				2000 // 放置物料
#define ARM_HIGH_SAVE			3.5f // 安全高度11

// 夹爪
#define CLAW_OPEN				2000 // 开
#define CLAW_CLOSE			1150 // 关
#define CLAW_START			2500	 // 起始位置
// 转盘
#define NUM1						800
#define NUM2						1500
#define NUM3						2200
// 平动
#define MOVE_OUT				2000 // 出手
#define MOVE_STOP				1500 // 停止
#define MOVE_IN					1000 // 收手
/* 动作组 */
#define ActionBuf_Num			40 // 数组内存大小
typedef struct 
{
	uint8_t* Flag; // 完成标志位记录标志
	float* SetTarget; // 控制目标值实际地址
	float target; // 目标值
}Act_Typedef;


// 机械臂
typedef struct
{
	uint8_t Actions_Done_Flag; // 动作组完成标志位
	uint8_t Vetical_Flag; // 升降到达目标值标志
	uint8_t Yaw_Flag; // 云台转向到达目标值标志
	uint8_t Turn_Flag;
	uint8_t Move_Flag; // 机械臂伸缩到达目标值标志
	uint8_t Move_dir; // 设置移动方向
	uint8_t Move_current_dis; //当前位置 // 0：内 1：外
	uint8_t Claw_Flag; // 夹爪关闭标志
	uint8_t Mode; // 模式
	float Set_Vetical_Dis; // 设置垂直距离
	float Set_Yaw_Dis; // 设置位置
	struct dj_motor_data* pYawMoto; // 底部云台
	struct dj_motor_data* pVeticalMoto; // 竖直电机
	struct SERVO_Device* pServo[3]; // 存放舵机数据
/*******************************************************/
	uint8_t current_act; // 记录当前到达运动组
	uint8_t act_num; // 记录一组动作个数
	Act_Typedef* Action[ActionBuf_Num]; // 记录动作组
} 
Arm_TypeDef;

/* 状态 */
enum ARM_MODE {
	START_MODE, // 起始模式
	IDLE_MODE, // 空闲模式
	FIND_MODE, // 找物料模式
	CATCH_FROM_TURN_TABLE, // 从转盘处抓取
};

/* 向Arm_Task 接收消息 */
typedef struct
{
	uint8_t ArmMode; // 模式
} Arm_Rx_Message_t;



#endif
